//##########################################################################
//#                                                                        #
//#                               CCLIB                                    #
//#                                                                        #
//#  This program is free software; you can redistribute it and/or modify  #
//#  it under the terms of the GNU Library General Public License as       #
//#  published by the Free Software Foundation; version 2 of the License.  #
//#                                                                        #
//#  This program is distributed in the hope that it will be useful,       #
//#  but WITHOUT ANY WARRANTY; without even the implied warranty of        #
//#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         #
//#  GNU General Public License for more details.                          #
//#                                                                        #
//#          COPYRIGHT: EDF R&D / TELECOM ParisTech (ENST-TSI)             #
//#                                                                        #
//##########################################################################
//
//*********************** Last revision of this file ***********************
//$Author::                                                                $
//$Rev::                                                                   $
//$LastChangedDate::                                                       $
//**************************************************************************
//

#include "ChunkedPointCloud.h"
#include "ScalarField.h"

#include <assert.h>

using namespace CCLib;

ChunkedPointCloud::ChunkedPointCloud()
{
	m_points = new GenericChunkedArray<3,PointCoordinateType>();
	m_points->link();

	clear();
}

ChunkedPointCloud::~ChunkedPointCloud()
{
	clear();
	m_points->release();
}

void ChunkedPointCloud::clear()
{
	m_points->clear();
	deleteAllScalarFields();
	placeIteratorAtBegining();
	invalidateBoundingBox();
}

unsigned ChunkedPointCloud::size() const
{
	return m_points->currentSize();
}

void ChunkedPointCloud::forEach(genericPointAction& anAction)
{
	unsigned i,n = size();

	//si un champ scalaire est actif
	ScalarField* currentOutScalarFieldArray = getCurrentOutScalarField();
	if (currentOutScalarFieldArray)
	{
		for (i=0;i<n;++i)
			anAction(*(CCVector3*)m_points->getValue(i),(*currentOutScalarFieldArray)[i]);
	}
	//sinon
	else
	{
		DistanceType dummyDist = 0.0;
		for (i=0;i<n;++i)
			anAction(*(CCVector3*)m_points->getValue(i),dummyDist);
	}
}

void ChunkedPointCloud::getBoundingBox(PointCoordinateType Mins[], PointCoordinateType Maxs[])
{
	if (!m_validBB)
	{
		m_points->computeMinAndMax();
		m_validBB = true;
	}

	memcpy(Mins,m_points->getMin(),3*sizeof(PointCoordinateType));
	memcpy(Maxs,m_points->getMax(),3*sizeof(PointCoordinateType));
}

void ChunkedPointCloud::invalidateBoundingBox()
{
    m_validBB = false;
}

void ChunkedPointCloud::placeIteratorAtBegining()
{
	m_currentPointIndex=0;
}

const CCVector3* ChunkedPointCloud::getNextPoint()
{
	return (m_currentPointIndex < m_points->currentSize() ? point(m_currentPointIndex++) : 0);
}

const CCVector3* ChunkedPointCloud::getPointPersistentPtr(unsigned index)
{
	return point(index);
}

void ChunkedPointCloud::getPoint(unsigned index, CCVector3& P) const
{
	assert(index < size());
	P=*point(index);
}

bool ChunkedPointCloud::resize(unsigned newNumberOfPoints)
{
	unsigned oldNumberOfPoints = m_points->currentSize();

	//we try to enlarge the 3D points array
	if (!m_points->resize(newNumberOfPoints))
        return false;

	//then the scalarfields
	for (unsigned i=0;i<m_scalarFields.size();++i)
	{
		//if something fails, we restore everything!
		if (!m_scalarFields[i]->resize(newNumberOfPoints))
        {
			for (unsigned j=0;j<i;++j)
                m_scalarFields[j]->resize(oldNumberOfPoints);
            m_points->resize(oldNumberOfPoints);
			return false;
        }
	}

	return true;
}

bool ChunkedPointCloud::reserve(unsigned newNumberOfPoints)
{
	//we try to enlarge the 3D points array
	if (!m_points->reserve(newNumberOfPoints))
        return false;

	//then the scalarfields
	for (unsigned i=0;i<m_scalarFields.size();++i)
	{
		if (!m_scalarFields[i]->reserve(newNumberOfPoints))
			return false;
	}

	return true;
}

void ChunkedPointCloud::addPoint(const CCVector3 &P)
{
	m_points->addElement(P.u);
	m_validBB = false;
}

void ChunkedPointCloud::applyTransformation(PointProjectionTools::Transformation& trans)
{
	unsigned i,count=size();

    if (trans.R.isValid())
    {
		for (i=0;i<count;++i)
			trans.R.apply(m_points->getValue(i));
        m_validBB = false;
    }

    if (trans.T.norm() > ZERO_TOLERANCE)
    {
		for (i=0;i<count;++i)
			*point(i) += trans.T;
        m_validBB = false;
    }
}

/***********************/
/***                 ***/
/***  SCALAR FIELDS  ***/
/***                 ***/
/***********************/

bool ChunkedPointCloud::isScalarFieldEnabled() const
{
    ScalarField* currentInScalarFieldArray = getCurrentInScalarField();
	if (!currentInScalarFieldArray)
        return false;

	unsigned sfValuesCount = currentInScalarFieldArray->currentSize();
    return (sfValuesCount>0 && sfValuesCount >= m_points->currentSize());
}

bool ChunkedPointCloud::enableScalarField()
{
    ScalarField* currentInScalarFieldArray = getCurrentInScalarField();

	if (!currentInScalarFieldArray)
	{
		//if we get there, it means that either the caller has forgot to create
		//(and assign) a scalar field to the cloud, or that we are in a compatibility
		//mode with old/basic behaviour: a unique SF for everything (input/output)

        //we look for any default scalar field already existing
		m_currentInScalarFieldIndex = getScalarFieldIndexByName("DefaultScalarField");
		if (m_currentInScalarFieldIndex < 0)
		{
            //if not, we create it
            m_currentInScalarFieldIndex = addScalarField("DefaultScalarField",true); //DGM: positive or not by default?!
            if (m_currentInScalarFieldIndex<0) //Something went wrong
                return false;
		}

        currentInScalarFieldArray = getCurrentInScalarField();
        assert(currentInScalarFieldArray);
	}

	//if there's no output scalar field either, we set this new scalar field as output also
	if (!getCurrentOutScalarField())
		m_currentOutScalarFieldIndex = m_currentInScalarFieldIndex;

	return currentInScalarFieldArray->resize(m_points->capacity());
}

void ChunkedPointCloud::setPointScalarValue(unsigned pointIndex, DistanceType value)
{
	assert(m_currentInScalarFieldIndex>=0 && m_currentInScalarFieldIndex<(int)m_scalarFields.size());
    //slow version
//    ScalarField* currentInScalarFieldArray = getCurrentInScalarField();
//    if (currentInScalarFieldArray)
//        currentInScalarFieldArray->setValue(pointIndex,&value);

    //fast version
    m_scalarFields[m_currentInScalarFieldIndex]->setValue(pointIndex,value);
}

DistanceType ChunkedPointCloud::getPointScalarValue(unsigned pointIndex) const
{
	assert(m_currentOutScalarFieldIndex>=0 && m_currentOutScalarFieldIndex<(int)m_scalarFields.size());
    //slow version
//    ScalarField* currentOutScalarFieldArray = getCurrentOutScalarField();
//    if (currentOutScalarFieldArray)
//        return *currentOutScalarFieldArray->getValue(pointIndex);
//    return BIG_VALUE;

    //fast version
    return m_scalarFields[m_currentOutScalarFieldIndex]->getValue(pointIndex);
}

ScalarField* ChunkedPointCloud::getScalarField(int index) const
{
    return (index>=0 && index<(int)m_scalarFields.size() ? m_scalarFields[index] : 0);
}

const char* ChunkedPointCloud::getScalarFieldName(int index) const
{
    return (index>=0 && index<(int)m_scalarFields.size() ? m_scalarFields[index]->getName() : 0);
}

int ChunkedPointCloud::addScalarField(const char* uniqueName, bool isStrictlyPositive)
{
    //we don't accept two SF with the same name!
    if (getScalarFieldIndexByName(uniqueName)>=0)
        return -1;

	//create requested scalar field
    ScalarField* sf = new ScalarField(uniqueName,isStrictlyPositive);
	if (!sf)
		return -1;

	//we don't want 'm_scalarFields' to grow by 50% each time! (default behavior of std::vector::push_back)
	try
	{
		m_scalarFields.push_back(sf);
	}
	catch (.../*const std::bad_alloc&*/) //out of memory
	{
		sf->release();
		return -1;
	}

	sf->link();

	return (int)m_scalarFields.size()-1;
}

void ChunkedPointCloud::deleteScalarField(int index)
{
	if (index<0 || m_scalarFields.empty())
        return;

    int sfCount = (int)m_scalarFields.size();
    if (index>=sfCount) //sfCount>0
        return;

    //we update SF roles if they point to the deleted scalar field
    if (index==m_currentInScalarFieldIndex)
        m_currentInScalarFieldIndex = -1;
    if (index==m_currentOutScalarFieldIndex)
        m_currentOutScalarFieldIndex = -1;

    //if the deleted SF is not the last one, we swap it with the last element
    int lastIndex = sfCount-1; //lastIndex>=0
    if (index < lastIndex) //i.e.lastIndex>0
    {
        std::swap(m_scalarFields[index],m_scalarFields[lastIndex]);
        //don't forget to update SF roles also if they point to the last element
        if (lastIndex==m_currentInScalarFieldIndex)
            m_currentInScalarFieldIndex = index;
        if (lastIndex==m_currentOutScalarFieldIndex)
            m_currentOutScalarFieldIndex = index;
    }

    //so we can always delete the last element (and the vector stays consistent)
	m_scalarFields.back()->release();
    m_scalarFields.pop_back();
}

void ChunkedPointCloud::deleteAllScalarFields()
{
    m_currentInScalarFieldIndex = m_currentOutScalarFieldIndex = -1;

	while (!m_scalarFields.empty())
	{
        m_scalarFields.back()->release();
		m_scalarFields.pop_back();
	}
}

int ChunkedPointCloud::getScalarFieldIndexByName(const char* name) const
{
    int i,sfCount=(int)m_scalarFields.size();
    for (i=0;i<sfCount;++i)
    {
        //we don't accept two SF with the same name!
        if (strcmp(m_scalarFields[i]->getName(),name)==0)
            return i;
    }

	return -1;
}

void ChunkedPointCloud::renameScalarField(int index, const char* name)
{
    ScalarField* sf = getScalarField(index);
    if (sf)
        sf->setName(name);
}

void ChunkedPointCloud::swapPoints(unsigned firstIndex, unsigned secondIndex)
{
	if (firstIndex==secondIndex || firstIndex>=m_points->currentSize() || secondIndex>=m_points->currentSize())
        return;

	m_points->swap(firstIndex,secondIndex);

    unsigned i,sfCount=m_scalarFields.size();
	for (i=0;i<sfCount;++i)
        m_scalarFields[i]->swap(firstIndex,secondIndex);
}
